/**
* Copyright (c) By zengqh.
*
* This program is just for fun or demo, in the hope that it  
* will be useful, you can redistribute it and/or modify freely.
*
* Time: 2013/02/18
* File: quaternion.cpp
**/

#include "enn_quaternion.h"

namespace enn
{
const Quaternion Quaternion::IDENTITY;

void Quaternion::FromAngleAxis(float angle, const vec3f& axis)
{
	vec3f normAxis = axis.Normalized();
	float sinAngle = sinf((angle * static_cast<float>ENN_DEGTORAD) * 0.5f);
	float cosAngle = cosf((angle * static_cast<float>ENN_DEGTORAD) * 0.5f);

	w_ = cosAngle;
	x_ = normAxis.x_ * sinAngle;
	y_ = normAxis.y_ * sinAngle;
	z_ = normAxis.z_ * sinAngle;
}

void Quaternion::FromEulerAngles(float x, float y, float z)
{
	// Order of rotations: Z first, then X, then Y (mimics typical FPS camera with gimbal lock at top/bottom)
	float sinX = sinf((x * static_cast<float>ENN_DEGTORAD) * 0.5f);
	float cosX = cosf((x * static_cast<float>ENN_DEGTORAD) * 0.5f);
	float sinY = sinf((y * static_cast<float>ENN_DEGTORAD) * 0.5f);
	float cosY = cosf((y * static_cast<float>ENN_DEGTORAD) * 0.5f);
	float sinZ = sinf((z * static_cast<float>ENN_DEGTORAD) * 0.5f);
	float cosZ = cosf((z * static_cast<float>ENN_DEGTORAD) * 0.5f);

	w_ = cosY * cosX * cosZ + sinY * sinX * sinZ;
	x_ = cosY * sinX * cosZ + sinY * cosX * sinZ;
	y_ = sinY * cosX * cosZ - cosY * sinX * sinZ;
	z_ = cosY * cosX * sinZ - sinY * sinX * cosZ;
}

void Quaternion::FromRotationTo(const vec3f& start, const vec3f& end)
{
	vec3f normStart = start.Normalized();
	vec3f normEnd = end.Normalized();
	float d = normStart.DotProduct(normEnd);

	if (d > -1.0f + ENN_EPSILON)
	{
		vec3f c = normStart.CrossProduct(normEnd);
		float s = sqrtf((1.0f + d) * 2.0f);
		float invS = 1.0f / s;

		x_ = c.x_ * invS;
		y_ = c.y_ * invS;
		z_ = c.z_ * invS;
		w_ = 0.5f * s;
	}
	else
	{
		vec3f axis = vec3f::RIGHT.CrossProduct(normStart);
		if (axis.Length() < ENN_EPSILON)
			axis = vec3f::UP.CrossProduct(normStart);
		float angle = 180.0f;

		vec3f normAxis = axis.Normalized();
		float sinAngle = sinf((angle * static_cast<float>ENN_DEGTORAD) * 0.5f);
		float cosAngle = cosf((angle * static_cast<float>ENN_DEGTORAD) * 0.5f);

		w_ = cosAngle;
		x_ = normAxis.x_ * sinAngle;
		y_ = normAxis.y_ * sinAngle;
		z_ = normAxis.z_ * sinAngle;
	}
}

void Quaternion::FromAxes(const vec3f& xAxis, const vec3f& yAxis, const vec3f& zAxis)
{
	Matrix3 matrix(
		xAxis.x_, yAxis.x_, zAxis.x_,
		xAxis.y_, yAxis.y_, zAxis.y_,
		xAxis.z_, yAxis.z_, zAxis.z_
		);

	FromRotationMatrix(matrix);
}

void Quaternion::FromRotationMatrix(const Matrix3& matrix)
{
	float t = matrix.m00_ + matrix.m11_ + matrix.m22_;

	if (t > 0.0f)
	{
		float s = 0.5f / sqrtf(1.0f + t);

		x_ = (matrix.m21_ - matrix.m12_) * s;
		y_ = (matrix.m02_ - matrix.m20_) * s;
		z_ = (matrix.m10_ - matrix.m01_) * s;
		w_ = 0.25f / s;
	}
	else
	{
		if (matrix.m00_ > matrix.m11_ && matrix.m00_ > matrix.m22_)
		{
			float s = sqrtf(1.0f + matrix.m00_ - matrix.m11_ - matrix.m22_) * 2.0f;
			float invS = 1.0f / s;

			x_ = 0.25f * s;
			y_ = (matrix.m01_ + matrix.m10_) * invS;
			z_ = (matrix.m20_ + matrix.m02_) * invS;
			w_ = (matrix.m21_ - matrix.m12_) * invS;
		}
		else if (matrix.m11_ > matrix.m22_)
		{
			float s = sqrtf(1.0f + matrix.m11_ - matrix.m00_ - matrix.m22_) * 2.0f;
			float invS = 1.0f / s;

			x_ = (matrix.m01_ + matrix.m10_) * invS;
			y_ = 0.25f * s;
			z_ = (matrix.m12_ + matrix.m21_) * invS;
			w_ = (matrix.m02_ - matrix.m20_) * invS;
		}
		else
		{
			float s = sqrtf(1.0f + matrix.m22_ - matrix.m00_ - matrix.m11_) * 2.0f;
			float invS = 1.0f / s;

			x_ = (matrix.m02_ + matrix.m20_) * invS;
			y_ = (matrix.m12_ + matrix.m21_) * invS;
			z_ = 0.25f * s;
			w_ = (matrix.m10_ - matrix.m01_) * invS;
		}
	}
}

vec3f Quaternion::EulerAngles() const
{
	// Derivation from http://www.geometrictools.com/Documentation/EulerAngles.pdf
	// Order of rotations: Z first, then X, then Y
	float check = 2.0f * (-y_ * z_ + w_ * x_);

	if (check < -0.995f)
	{
		return vec3f(
			-90.0f,
			0.0f,
			-atan2f(2.0f * (x_ * z_ - w_ * y_), 1.0f - 2.0f * (y_ * y_ + z_ * z_)) * static_cast<float>ENN_RADTODEG
			);
	}
	else if (check > 0.995f)
	{
		return vec3f(
			90.0f,
			0.0f,
			atan2f(2.0f * (x_ * z_ - w_ * y_), 1.0f - 2.0f * (y_ * y_ + z_ * z_)) * static_cast<float>ENN_RADTODEG
			);
	}
	else
	{
		return vec3f(
			asinf(check) * static_cast<float>ENN_RADTODEG,
			atan2f(2.0f * (x_ * z_ + w_ * y_), 1.0f - 2.0f * (x_ * x_ + y_ * y_)) * static_cast<float>ENN_RADTODEG,
			atan2f(2.0f * (x_ * y_ + w_ * z_), 1.0f - 2.0f * (x_ * x_ + z_ * z_)) * static_cast<float>ENN_RADTODEG
			);
	}
}

float Quaternion::YawAngle() const
{
	return EulerAngles().y_;
}

float Quaternion::PitchAngle() const
{
	return EulerAngles().x_;
}

float Quaternion::RollAngle() const
{
	return EulerAngles().z_;
}

Matrix3 Quaternion::RotationMatrix() const
{
	return Matrix3(
		1.0f - 2.0f * y_ * y_ - 2.0f * z_ * z_,
		2.0f * x_ * y_ - 2.0f * w_ * z_,
		2.0f * x_ * z_ + 2.0f * w_ * y_,
		2.0f * x_ * y_ + 2.0f * w_ * z_,
		1.0f - 2.0f * x_ * x_ - 2.0f * z_ * z_,
		2.0f * y_ * z_ - 2.0f * w_ * x_,
		2.0f * x_ * z_ - 2.0f * w_ * y_,
		2.0f * y_ * z_ + 2.0f * w_ * x_,
		1.0f - 2.0f * x_ * x_ - 2.0f * y_ * y_
		);
}

Quaternion Quaternion::Slerp(Quaternion rhs, float t) const
{
	float cosAngle = DotProduct(rhs);
	// Enable shortest path rotation
	if (cosAngle < 0.0f)
	{
		cosAngle = -cosAngle;
		rhs = -rhs;
	}

	float angle = acosf(cosAngle);
	float sinAngle = sinf(angle);
	float t1, t2;

	if (sinAngle > 0.001f)
	{
		float invSinAngle = 1.0f / sinAngle;
		t1 = sinf((1.0f - t) * angle) * invSinAngle;
		t2 = sinf(t * angle) * invSinAngle;
	}
	else
	{
		t1 = 1.0f - t;
		t2 = t;
	}

	return *this * t1 + rhs * t2;
}
}